﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectDetection.cs
 * \brief	This file contains a class with basic mathematical methods for the
 * M44 SensorFusionAndMapping module.
 *  
 * This file contains all the basic mathematical methods needed by the DataMapping
 * module
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.02
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-11-20 \n
 *      First implementation of the M44_ObjectDetection class.
 *      
 * -Version 0.02 Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 *      
 *      
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/* M44_SharedClasses */
using NMRS.M44_SharedClasses;

/*These are for plotting files*/
using System.Globalization;
using System.IO;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ObjectDetection
     * 
     *  \brief  This is the class of the M44_ObjectDetection function.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_ObjectDetection
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static DetectedObjects ObjectDetection( cl_3DLrfMeasure Lrf3DMeasure,
                                                               cl_3DLrfMeasure LastLrf3DMeasure
                                                              )
         * 
         *  \brief This function finds the correct cell in the matrix that represents 'Map'
         *   of the coordinates [x,y]
         *  
         *  \param[in] cl_3DLrfMeasure Lrf3DMeasure
         *  \param[in] cl_3DLrfMeasure LastLrf3DMeasure
         *  
         *  \param[out] DetectedObjects str_DetectedObjects         Detected Objects structture
         *
         */
        /* ------------------------------------------------------------------------- */
        public static DetectedObjects ObjectDetection( cl_3DLrfMeasure Lrf3DMeasure,
                                                       cl_3DLrfMeasure LastLrf3DMeasure
                                                      )
        {
            /*Variables*/
            DetectedObjects str_DetectedObjects   = new DetectedObjects(Lrf3DMeasure.s_i, Lrf3DMeasure.s_n);
            float[,]        tf_DistanceDiference  = new float[Lrf3DMeasure.s_i, Lrf3DMeasure.s_n ];
            float[,]        tf_Objects            = new float[Lrf3DMeasure.s_i, Lrf3DMeasure.s_n];
            float f_difference_tol = DynObj_Const.f_laser_laser_dif_tol;
            float f_percentage_tol = DynObj_Const.f_laser_scan_percentage_tol;
            int             i_nzero               = 0;  //number of elements in the difference that are zero
            
  
            /*Filter points with difference too small*/
            for (int i = 0; i < Lrf3DMeasure.s_i; i++)
            {
                for (int j = 0; j < Lrf3DMeasure.s_n; j++)
                {
                    tf_Objects[i,j] = 0.0f;
                    tf_DistanceDiference[i,j] = Lrf3DMeasure.tf_3DLrfDistance[i,j] - LastLrf3DMeasure.tf_3DLrfDistance[i,j];

                    if (Math.Abs(tf_DistanceDiference[i, j]) < f_difference_tol)
                    {
                        tf_DistanceDiference[i, j] = 0;
                    }

                    if (tf_DistanceDiference[i, j] == 0)
                    {
                        i_nzero++;
                    }

                }
            }
            if (i_nzero > f_percentage_tol * Lrf3DMeasure.s_i * Lrf3DMeasure.s_n)
            {
                str_DetectedObjects = M44_ObjectFinder.ObjectFinder(tf_DistanceDiference, Lrf3DMeasure.s_i, Lrf3DMeasure.s_n);
                str_DetectedObjects.i_status = 1;
            }         
            return str_DetectedObjects;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public void CreatePlotFile(string RobotId, string ListType, cl_DynamicObjects DynamicObjects)
         *
         *  \brief It generates a data file to plot Dynamic Objects List.
         *  
         */
        /* ------------------------------------------------------------------------- */
        public static void CreatePlotFile(string RobotId, cl_DynamicObjects DynamicObjects, string ListType)
        {

            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;

            /*It creates filter.m file and stores the desired values*/
            for (int i = 0; i < DynamicObjects.List.Length; i++)
            {
                using (StreamWriter w = File.AppendText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\" + RobotId + ListType + ".m"))
                {
                    
                    w.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}",
                            DynamicObjects.List[i].f_DetectionTime.ToString("N", nfi),
                            DynamicObjects.List[i].us_Identification.ToString("N", nfi),
                            DynamicObjects.List[i].Position.d_x.ToString("N", nfi),
                            DynamicObjects.List[i].Position.d_y.ToString("N", nfi),
                            DynamicObjects.List[i].Velocity.d_xVel.ToString("N", nfi),
                            DynamicObjects.List[i].Velocity.d_yVel.ToString("N", nfi)
                            );

                    /*Update the underlying file.*/
                    w.Flush();

                    /*Close the writer and underlying file.*/
                    w.Close();
                }
            }
        }
    }
}
